#ifndef _AMERICO_VECTOR3_H_
#define _AMERICO_VECTOR3_H_

#include <cassert>
#include <cmath>
#include "staticarray.h"

template <typename TYPE>
class Vector3
{
public:
    union
    {
        TYPE myX, x;
        TYPE myR, r;
    };
    union
    {
        TYPE myY, y;
        TYPE myG, g;
    };
    union
    {
        TYPE myZ, z;
        TYPE myB, b;
    };

    Vector3();
    Vector3(const TYPE &aX, const TYPE &aY, const TYPE &aZ);
    ~Vector3();

    Vector3 operator-() const;
    Vector3 operator+(const Vector3 &aVector) const;
    Vector3 operator-(const Vector3 &aVector) const;
    Vector3 operator*(const Vector3 &aVector) const;
    Vector3 operator*(const TYPE &aScalar) const;
    Vector3 operator/(const TYPE &aScalar) const;

    Vector3 &operator+=(const Vector3 &aVector);
    Vector3 &operator-=(const Vector3 &aVector);
    Vector3 &operator*=(const Vector3 &aVector);
    Vector3 &operator*=(const TYPE &aScalar);
    Vector3 &operator/=(const TYPE &aScalar);

	bool operator==(const Vector3 &aVector) const;
	bool operator!=(const Vector3 &aVector) const;

	Vector3 &Set(const TYPE &aX, const TYPE &aY, const TYPE &aZ);

    float Length() const;
	float Length2() const;
    TYPE Dot(const Vector3 &aVector) const;
	Vector3 Cross(const Vector3 &aVector) const;

	bool GoodEnough(const Vector3& aVector, const float& aGoodEnoughValue = 0.01f);

	Vector3 &Normalize();
	Vector3 Normalize() const;
};



template <typename TYPE>
Vector3<TYPE>::Vector3()
{
}

template <typename TYPE>
Vector3<TYPE>::Vector3(const TYPE &aX, const TYPE &aY, const TYPE &aZ)
{
    myX = aX;
    myY = aY;
    myZ = aZ;
}

template <typename TYPE>
Vector3<TYPE>::~Vector3()
{
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator-() const
{
    Vector3<TYPE> result(-myX, -myY, -myZ);
    return result;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator+(const Vector3<TYPE> &aVector) const
{
    Vector3<TYPE> result(*this);
    result += aVector;
    return result;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator-(const Vector3<TYPE> &aVector) const
{
    Vector3<TYPE> result(*this);
    result -= aVector;
    return result;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator*(const Vector3<TYPE> &aVector) const
{
    Vector3<TYPE> result(*this);
    result *= aVector;
    return result;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator*(const TYPE &aScalar) const
{
    Vector3<TYPE> result(*this);
    result *= aScalar;
    return result;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::operator/(const TYPE &aScalar) const
{
    Vector3<TYPE> result(*this);
    result /= aScalar;
    return result;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::operator+=(const Vector3<TYPE> &aVector)
{
    myX += aVector.myX;
    myY += aVector.myY;
    myZ += aVector.myZ;
    return *this;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::operator-=(const Vector3<TYPE> &aVector)
{
    myX -= aVector.myX;
    myY -= aVector.myY;
    myZ -= aVector.myZ;
    return *this;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::operator*=(const Vector3<TYPE> &aVector)
{
    myX *= aVector.myX;
    myY *= aVector.myY;
    myZ *= aVector.myZ;
    return *this;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::operator*=(const TYPE &aScalar)
{
    myX *= aScalar;
    myY *= aScalar;
    myZ *= aScalar;
    return *this;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::operator/=(const TYPE &aScalar)
{
	assert(aScalar!=0 && "Division by 0");
    myX /= aScalar;
    myY /= aScalar;
    myZ /= aScalar;
    return *this;
}

template <typename TYPE>
bool Vector3<TYPE>::operator==(const Vector3<TYPE> &aVector) const
{
	return ((myX == aVector.myX) && (myY == aVector.myY) && (myZ == aVector.myZ));
}

template <typename TYPE>
bool Vector3<TYPE>::operator!=(const Vector3<TYPE> &aVector) const
{
	return ! operator==(aVector);
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::Set(const TYPE &aX, const TYPE &aY, const TYPE &aZ)
{
	myX = aX;
	myY = aY;
	myZ = aZ;
	return *this;
}

template <typename TYPE>
float Vector3<TYPE>::Length() const
{
    return sqrt(myX*myX+myY*myY+myZ*myZ);
}

template <typename TYPE>
float Vector3<TYPE>::Length2() const
{
	return (myX*myX+myY*myY+myZ*myZ);
}

template <typename TYPE>
TYPE Vector3<TYPE>::Dot(const Vector3<TYPE> &aVector) const
{
    return (myX*aVector.myX+myY*aVector.myY+myZ*aVector.myZ);
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::Cross(const Vector3<TYPE> &aVector) const
{
    Vector3<TYPE> result;
    result.myX = myY*aVector.myZ - myZ*aVector.myY;
    result.myY = myZ*aVector.myX - myX*aVector.myZ;
    result.myZ = myX*aVector.myY - myY*aVector.myX;
    return result;
}

template <typename TYPE>
Vector3<TYPE> &Vector3<TYPE>::Normalize()
{
    TYPE length = Length();
	if(length>0)
	{
		myX /= length;
		myY /= length;
		myZ /= length;
	}
	return *this;
}

template <typename TYPE>
Vector3<TYPE> Vector3<TYPE>::Normalize() const
{
	TYPE length = Length();
	Vector3<TYPE> res(*this);
	if(length>0)
	{
		res.myX /= length;
		res.myY /= length;
		res.myZ /= length;
	}
	return res;
}

template <typename TYPE>
bool Vector3<TYPE>::GoodEnough( const Vector3& aVector, const float& aGoodEnoughValue /*= 0.01f*/ )
{
	Vector3<TYPE> tempMe = *this;
	Vector3<TYPE> tempYou = aVector;

	Vector3<TYPE> difference = abs(tempMe) - abs(tempYou);
	abs(difference);

	if( difference.Length() <= aGoodEnoughValue )
	{
		return true;
	}
	return false;
}
#endif
